{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import yaml\n",
    "import os\n",
    "import csv\n",
    "import math\n",
    "import matplotlib.pyplot as plt\n",
    "from collections import deque\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def read_last_xy_from_files(folder):\n",
    "    x_list = list()\n",
    "    y_list = list()\n",
    "    \n",
    "    # loop over all files in folder\n",
    "    for filename in os.listdir(folder):\n",
    "        if filename.endswith(\".csv\"):\n",
    "            # open csv file\n",
    "            with open(folder + filename, 'r') as f:\n",
    "                try:\n",
    "                    # get last values\n",
    "                    lastrow = deque(csv.reader(f), 1)[0]\n",
    "                    x = float(lastrow[1])         \n",
    "                    y = float(lastrow[2])  \n",
    "                    \n",
    "                    # store in list\n",
    "                    x_list.append(-x) \n",
    "                    y_list.append(-y)\n",
    "\n",
    "                except IndexError:  # empty file\n",
    "                    lastrow = None\n",
    "    return x_list, y_list"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# CW\n",
    "err_x_CW, err_y_CW = read_last_xy_from_files(\"./data_front_022/CW/\")\n",
    "\n",
    "# CCW\n",
    "err_x_CCW, err_y_CCW = read_last_xy_from_files(\"./data_front_022/CCW/\")\n",
    "\n",
    "# CW COG\n",
    "x_cg_CW = sum(err_x_CW)/len(err_x_CW)\n",
    "y_cg_CW = sum(err_y_CW)/len(err_y_CW)\n",
    "\n",
    "# CCW COG\n",
    "x_cg_CCW = sum(err_x_CCW)/len(err_x_CCW)\n",
    "y_cg_CCW = sum(err_y_CCW)/len(err_y_CCW)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# length of square\n",
    "L = 13.03\n",
    "\n",
    "# used trackwidth\n",
    "b = 0.22\n",
    "\n",
    "# calculate E_b (flipped sign because of different coordinate system)\n",
    "alpha_1 = (x_cg_CW + x_cg_CCW)/(-4.0*L)\n",
    "alpha_2 = (y_cg_CW - y_cg_CCW)/(-4.0*L)\n",
    "alpha =  (alpha_1 + alpha_2)/2.0\n",
    "E_b = (math.pi/2)/(math.pi/2 - alpha) \n",
    "\n",
    "# calculate E_d (flipped sign because of different coordinate system)\n",
    "beta_1 = (x_cg_CW - x_cg_CCW)/(-4.0*L)\n",
    "beta_2 = (y_cg_CW + y_cg_CCW)/(-4.0*L)\n",
    "beta = (beta_1 + beta_2)/2.0\n",
    "R = (L/2.0)/math.sin(beta/2.0)\n",
    "E_d = (R + b/2.0)/(R - b/2.0)\n",
    "\n",
    "# print\n",
    "print(\"Error E_d: \", E_d)\n",
    "print(\"Error E_b: \", E_b)\n",
    "print(\"Actual track width b_actual: \", E_b * b)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# corrected CW\n",
    "err_x_CW_corr, err_y_CW_corr = read_last_xy_from_files(\"./data_result/CW/\")\n",
    "\n",
    "# corrected CCW\n",
    "err_x_CCW_corr, err_y_CCW_corr = read_last_xy_from_files(\"./data_result/CCW/\")\n",
    "\n",
    "# CW COG corrected\n",
    "x_cg_CW_corr = sum(err_x_CW_corr)/len(err_x_CW_corr)\n",
    "y_cg_CW_corr = sum(err_y_CW_corr)/len(err_y_CW_corr)\n",
    "\n",
    "# CCW COG corrected\n",
    "x_cg_CCW_corr = sum(err_x_CCW_corr)/len(err_x_CCW_corr)\n",
    "y_cg_CCW_corr = sum(err_y_CCW_corr)/len(err_y_CCW_corr)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "fig, ax = plt.subplots()\n",
    "\n",
    "# negate elements\n",
    "err_x_CCW = [-x for x in err_x_CCW]\n",
    "err_y_CCW = [-x for x in err_y_CCW]\n",
    "err_x_CW = [-x for x in err_x_CW]\n",
    "err_y_CW = [-x for x in err_y_CW]\n",
    "x_cg_CW = - x_cg_CW\n",
    "x_cg_CCW = - x_cg_CCW\n",
    "y_cg_CW = - y_cg_CW\n",
    "y_cg_CCW = - y_cg_CCW\n",
    "\n",
    "# intermediate results\n",
    "ax.scatter(err_x_CCW, err_y_CCW, color=\"green\", label=\"CCW\")\n",
    "ax.scatter(err_x_CW, err_y_CW, color=\"blue\", label=\"CW\")\n",
    "ax.scatter([x_cg_CW, x_cg_CCW], [y_cg_CW, y_cg_CCW], color=\"red\", marker=\"+\", label=\"COG\")\n",
    "\n",
    "# negate elements\n",
    "err_x_CCW_corr = [-x for x in err_x_CCW_corr]\n",
    "err_y_CCW_corr = [-x for x in err_y_CCW_corr]\n",
    "err_x_CW_corr = [-x for x in err_x_CW_corr]\n",
    "err_y_CW_corr = [-x for x in err_y_CW_corr]\n",
    "x_cg_CW_corr = - x_cg_CW_corr\n",
    "x_cg_CCW_corr = - x_cg_CCW_corr\n",
    "y_cg_CW_corr = - y_cg_CW_corr\n",
    "y_cg_CCW_corr = - y_cg_CCW_corr\n",
    "\n",
    "\n",
    "# corrected results\n",
    "ax.scatter(err_x_CCW_corr, err_y_CCW_corr, color=\"goldenrod\", marker=\"s\", label=\"CCW corrected\")\n",
    "ax.scatter(err_x_CW_corr, err_y_CW_corr, color=\"sienna\", marker=\"s\", label=\"CW corrected\")\n",
    "ax.scatter([x_cg_CW_corr, x_cg_CCW_corr], [y_cg_CW_corr, y_cg_CCW_corr], color=\"red\", marker=\"x\", label=\"COG corrected\")\n",
    "\n",
    "\n",
    "# format everything\n",
    "ax.set_xlabel('x_err in m')\n",
    "ax.set_ylabel('y_err in m')\n",
    "\n",
    "# set the x-spine (see below for more info on `set_position`)\n",
    "ax.spines['left'].set_position('zero')\n",
    "\n",
    "# turn off the right spine/ticks\n",
    "ax.spines['right'].set_color('none')\n",
    "ax.yaxis.tick_left()\n",
    "\n",
    "# set the y-spine\n",
    "ax.spines['bottom'].set_position('zero')\n",
    "\n",
    "# turn off the top spine/ticks\n",
    "ax.spines['top'].set_color('none')\n",
    "ax.xaxis.tick_bottom()\n",
    "\n",
    "# make axis equal\n",
    "ax.axis('equal')\n",
    "\n",
    "# legend\n",
    "fig.legend()\n",
    "\n",
    "fig.savefig(\"result.svg\", format=\"svg\")\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
